﻿#include <iostream>
#include <algorithm>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;

/// <summary>
/// 查找轮廓
/// </summary>
/// <param name="image"></param>
/// <param name="contours"></param>
void findContours(Mat& image, std::vector<std::vector<cv::Point>>& contours) {
    // 转换为灰度图像
    cv::Mat gray;
    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

    // 高斯模糊
    cv::Mat blurred;
    cv::GaussianBlur(gray, blurred, cv::Size(5, 5), 0);
    cv::waitKey(0);
    // 边缘检测
    cv::Mat edged;
    cv::Canny(blurred, edged, 50, 200);

    // 寻找轮廓
    cv::findContours(edged, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);
}

// 计算P0 X+Y最小的
void findTopLeft(std::vector<cv::Point>& approx, cv::Point2f& p) {
    for (int i = 0; i < approx.size(); ++i) {
        cv::Point temp = approx[i];
        if ((p.x == 0) || (p.x + p.y) > (temp.x + temp.y)) {
            p.x = temp.x;
            p.y = temp.y;
        }
    }
}

// 计算P2 x+y最大的
void findBottomRight(std::vector<cv::Point>& approx, cv::Point2f& p) {
    for (int i = 1; i < approx.size(); ++i) {
        cv::Point temp = cv::Point(approx[i].x, approx[i].y);
        if ((p.x == 0) || (p.x + p.y) < (temp.x + temp.y)) {
            p.x = temp.x;
            p.y = temp.y;
        }
    }
}

// 计算P1  除P0和P2外 X最大的
void maxXMargin(std::vector<cv::Point>& approx, cv::Point2f& p0, cv::Point2f& p2, cv::Point2f& p) {
    for (int i = 0; i < approx.size(); ++i) {
        cv::Point temp = approx[i];
        if (temp.x == p0.x && temp.y==p0.y)
        {
            continue;
        }
        if (temp.x == p2.x && temp.y == p2.y) {
            continue;
        }

        if ((p.x==0)||(p.x ) < (temp.x )) {
            p.x = temp.x;
            p.y = temp.y;
        }
    }
}

void exceptPoint(std::vector<cv::Point>& approx, cv::Point2f& p0, cv::Point2f& p2, cv::Point2f& p1, cv::Point2f& p) {
    for (int i = 0; i < approx.size(); ++i) {
        cv::Point temp = approx[i];
        if (temp.x == p0.x && temp.y == p0.y){
            continue;
        }
        if (temp.x == p2.x && temp.y == p2.y) {
            continue;
        }
        if (temp.x == p1.x && temp.y == p1.y) {
            continue;
        }

        p.x = temp.x;
        p.y = temp.y;
    }
}

// 从旋转矩阵中提取俯仰角（Pitch angle）
double getPitchAngleFromRotationMatrix(const cv::Mat& R) {
    double pitch = atan2(-R.at<double>(2, 0), sqrt(R.at<double>(2, 1) * R.at<double>(2, 1) + R.at<double>(2, 2) * R.at<double>(2, 2)));
    return pitch * 180.0 / CV_PI;
}

/// <summary>
/// 提取平移和旋转
/// </summary>
/// <param name="H"></param>
/// <param name="R"></param>
/// <param name="t"></param>
void decomposeHomography(const cv::Mat& H, cv::Mat& R, cv::Mat& t) {
    // 提取旋转和平移信息
    cv::SVD svd(H(cv::Rect(0, 0, 3, 3)));
    R = svd.u * svd.vt; // 旋转矩阵
    t = H.col(2); // 平移向量
}

/// <summary>
/// 放大四边形
/// </summary>
/// <param name="rect">四边形</param>
/// <param name="scale">放大比例</param>
void magnifyRect(cv::Mat image,std::vector<cv::Point2f> srcPoints,double scale, std::vector<cv::Point2f>& desPoints) {
    // 计算四边形中心点
    cv::Point2f center(0, 0);
    for (cv::Point2f pt : srcPoints) {
        center += pt;
    }
    center *= (1.0 / srcPoints.size());

    std::vector<cv::Point2f> tempList;
    for (cv::Point2f pt : srcPoints) {
        cv::Point2f direction = pt - center;
        cv::Point2f temp = (center + direction * scale);
        tempList.push_back(temp);
    }

    int width = image.cols;
    int height = image.rows;

    // 把点平移到图片范围之内
    float xmove=0, ymove=0;
    for (cv::Point2f des : tempList) {
        if (des.x < 0 && xmove<(0 - des.x)) {
            xmove = 0 - des.x;
        }
        if (des.x > width && xmove >(width - des.x)) {
            xmove = width - des.x;
        }

        if (des.y < 0 && ymove <(0 - des.x)) {
            ymove = 0 - des.x;
        }
        if (des.y > height && ymove <(height - des.y)) {
            ymove = height - des.y;
        }
    }
    // 如果超出图片范围，那么就平移
    for (cv::Point2f t : tempList) {
        cv::Point2f des(t.x + xmove, t.y + ymove);
        desPoints.push_back(des);
    }

}

/// <summary>
/// 查找正方形并变换
/// </summary>
/// <param name="rawImg"></param>
/// <param name="contours"></param>
/// <param name="warped"></param>
double findSquareAndTransfer(Mat& rawImg, std::vector<std::vector<cv::Point>>& contours, cv::Mat& warped,float scale) {
    // 遍历轮廓寻找正方形
    for (const auto& contour : contours) {
        // 近似多边形
        std::vector<cv::Point> approx;
        double epsilon = 0.02 * cv::arcLength(contour, true);
        cv::approxPolyDP(contour, approx, epsilon, true);

        // 如果是四边形
        if (approx.size() == 4) {
            // 将顶点排序
            std::vector<cv::Point2f> rect(4);
            std::vector<cv::Point2f> from;
            std::vector<cv::Point2f> dst(4);

            findTopLeft(approx, rect[0]);     // top-left
            findBottomRight(approx, rect[2]);  // bottom-right
            maxXMargin(approx, rect[0], rect[2], rect[1]);  // top-right
            exceptPoint(approx, rect[0], rect[2], rect[1], rect[3]);  // bottom-left

            magnifyRect(rawImg,rect, scale,from);
               
            // 目标矩形的顶点 这里给固定值是为了保证多张图变换后标定大小相同
            float width = 100* scale;//std::abs(rect[1].x - rect[0].x);
            dst[0] = cv::Point2f(from[0].x, from[0].y);
            dst[1] = cv::Point2f(from[0].x + width, from[0].y);
            dst[2] = cv::Point2f(from[0].x + width, from[0].y + width);
            dst[3] = cv::Point2f(from[0].x, from[0].y + width);

            // 计算透视变换矩阵
            cv::Mat M = cv::getPerspectiveTransform(from, dst);

            // 应用透视变换
            cv::warpPerspective(rawImg, warped, M, cv::Size(rawImg.cols, rawImg.rows));

            // 显示矫正后的图像
            cv::imshow("Warped", warped);
            cv::waitKey(0);
            cv::destroyAllWindows();

            cv::Mat R, t;
            decomposeHomography(M, R, t);

            double angle = getPitchAngleFromRotationMatrix(R);
            return angle;
            //break;  // 只处理找到的第一个正方形
        }
    }
    return 0;
}

/// <summary>
/// 计算直线的长度
/// </summary>
/// <param name="contours"></param>
/// <returns></returns>
double calcLineLength(cv::Mat& warped) {
    std::vector<std::vector<cv::Point>> contours;
    findContours(warped, contours);

    // 遍历轮廓
    for (size_t i = 0; i < contours.size(); i++) {
        // 使用多边形逼近轮廓
        std::vector<cv::Point> approx;
        double epsilon = 0.02 * cv::arcLength(contours[i], true);
        cv::approxPolyDP(contours[i], approx, epsilon, true);

        // 如果逼近后有两个顶点，则认为是直线
        if (approx.size() == 2) {
            cv::Point pt1 = approx[0];
            cv::Point pt2 = approx[1];

            // 计算直线长度
            double length = cv::norm(pt1 - pt2);

            return length;
        }
    }
    return 0;
}

double calcLineLength2(cv::Mat& wraped) {
    // 将图像转换为灰度图像
    cv::Mat gray;
    cv::cvtColor(wraped, gray, cv::COLOR_BGR2GRAY);

    // 边缘检测
    cv::Mat edges;
    cv::Canny(gray, edges, 50, 150, 3);

    // 使用霍夫直线变换检测直线
    std::vector<cv::Vec4i> lines;
    cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 50, 50, 10);

    if (lines.empty()) {
        std::cout << "No lines detected!" << std::endl;
        return -1;
    }
    double length = 0;
    for (int i = 0; i < lines.size(); i++) {
        cv::Vec4i l = lines[i];
        cv::Point pt1(l[0], l[1]); // 直线起点
        cv::Point pt2(l[2], l[3]); // 直线终点

        double temp = cv::norm(pt1 - pt2);
        if (180 < temp < 220) {
            length = temp;
        }
    }
    return length;
}



int main()
{
    // 读取图像
    Mat image = imread("d:\\x1.png");
  /*  if (image.empty()) {
        std::cerr << "Could not open or find the image!" << std::endl;
        return -1;
    }*/

    // 寻找轮廓
    std::vector<std::vector<cv::Point>> contours;
    findContours(image, contours);

    //查找正方形并变换
    cv::Mat warped1,warpedScale;
    double turnAngle = findSquareAndTransfer(image, contours, warped1, 1);
    findSquareAndTransfer(image, contours, warpedScale,2.8);
    double wrapedLineLeng = calcLineLength(warpedScale);

    std::cout << "turned:"<< turnAngle<<" wraped line length:"<<wrapedLineLeng<<endl;

    return 0;
}